#include "slam_app.h"

#include "slam.h"
#include "mapviewer.h"
#include <aruco/posetracker.h>
#include <Eigen/Geometry>
#include <opencv2/imgproc/imgproc_c.h>
#include <thread>
ucoslam::ImageParams *image_params;
ucoslam::Slam *pSlam;
slam_app::slam_app()
{

    std::fstream fss("../2023_05_09_16_03_33/file.txt", std::ios::in);
    // cv::FileStorage fs(" ../2023_05_09_16_05_02/pose.yml", cv::FileStorage::READ);
    // cv::Mat idx;
    // fs["idx"] >> idx;
    // std::cout << idx << std::endl;

    for (int i = 0; fss.good(); ++i)
    {
        string s1;
        fss >> s1;
        if (s1.size() > 0)
            imgpaths.push_back("../2023_05_09_16_03_33" + s1);
        else
            break;
    }
    cout << imgpaths.size() << endl;
    // as.pop_back();
    fss.close();

    ucoslam::Params params;
    image_params = new ucoslam::ImageParams();
    image_params->readFromXMLFile("../MarkerSLAM/camera.yml");
    params.readFromYMLFile("../MarkerSLAM/arucoConfig1.yml");
    pSlam = new ucoslam::Slam();
    auto TheMap = std::make_shared<ucoslam::Map>();

    pSlam->setParams(TheMap, params);
    cout << "===@params=== \n";
    cout << "Marker size " << params.aruco_markerSize << endl;
    cout << "min distance " << params.minBaseLine << endl;
    cout << "min err Ratio " << params.aruco_minerrratio_valid << endl;
    cout << "fx=" << image_params->fx() << ", fy=" << image_params->fy() << endl;
    cout << "cx=" << image_params->cx() << ", cy=" << image_params->cy() << endl;
    cout << "distorsion = \n"
         << image_params->Distorsion << endl;
    // Ok, lets start
}
void slam_app::run(cv::Mat &inimg, cv::Mat &in_depth)
{
    if (j >= imgpaths.size() / 2)
        return;
    {
        int currentFrameIndex = j;
        cout << "fi:" << j << imgpaths[j * 2] << endl;
        // cv::Mat inimg = cv::imread(imgpaths[j * 2 + 1]);
        in_image = inimg.clone();
        // in_depth = cv::imread(imgpaths[j * 2], -1);
        curr_pose = pSlam->process_input(inimg, *image_params, currentFrameIndex, in_depth);
        if (!curr_pose.empty())
        {
            paths.push_back(cv::Affine3f(curr_pose));
            ALLpaths_rgbs.push_back(curr_pose);
            paths_rgbs.push_back(imgpaths[j * 2 + 1]);
            paths_deps.push_back(imgpaths[j * 2]);
        }
    }
    j++;
    // cv::Mat p1, pi;
    // for (auto &it : pSlam->TheMap->keyframes)
    // {
    //     cv::Mat p = it.pose_f2g.inv();
    //     p1.push_back(p);
    //     pi.push_back(cv::Vec3i(it.fseq_idx));
    //     // cout << it.fseq_idx << ":" << p << endl;
    // }
    // cv::FileStorage fs22("mpose4.yml", cv::FileStorage::WRITE);
    // fs22 << "pose" << p1;
    // fs22 << "idx" << pi;
    // fs22 << "paths_rgbs" << paths_rgbs;
    // fs22 << "paths_deps" << paths_deps;
    // fs22 << "ALLpaths_rgbs" << ALLpaths_rgbs;
    // fs22.release();
}

cv::Mat slam_app::savePosesToFile(string filename)
{
    std::ofstream file(filename);
    cv::Mat ps;
    // for (auto frame : fmp)
    // {
    //     if (!frame.second.empty())
    //     {
    //         cv::Mat minv = frame.second.inv();
    //         ps.push_back(minv);
    //         float *p = minv.ptr<float>();
    //         file << frame.first << endl;
    //     }
    // }
    return ps;
}
slam_app::~slam_app()
{
}
